/*
 * collision_detector.h
 *
 *  Created on: Apr 7, 2010
 *      Author: Aron
 */
//working version

#ifndef COLLISION_DETECTOR_H_
#define COLLISION_DETECTOR_H_

#include "xtask.h"
#include "sensor_read.h"

using namespace kthrobot;
using namespace std;

namespace Group5 {

    class IRManager {
    public:
        IRManager(SensorsReader* sensors_reader) : sensors_reader_(sensors_reader) {};

        // Primitive methods to get the ranges from the sensors
        int FrontRange()        { return sensors_reader_->GetDistanceCm(FRONT); }
        int RearRange()         { return sensors_reader_->GetDistanceCm(REAR); }
        int FrontLeftRange()    { return sensors_reader_->GetDistanceCm(FRONTLEFT); }
        int RearLeftRange()     { return sensors_reader_->GetDistanceCm(REARLEFT); }
        int FrontRightRange()   { return sensors_reader_->GetDistanceCm(FRONTRIGHT); }
        int RearRightRange()    { return sensors_reader_->GetDistanceCm(REARRIGHT); }

        // (slightly) Smarter methods: tell if there is free space detected
        bool FrontLeftFree()    { return sensors_reader_->GetDistanceCm(FRONTLEFT) > SIDERANGE; }
        bool RearLeftFree()     { return sensors_reader_->GetDistanceCm(REARLEFT) > SIDERANGE; }
        bool FrontRightFree()   { return sensors_reader_->GetDistanceCm(FRONTRIGHT) > SIDERANGE; }
        bool RearRightFree()    { return sensors_reader_->GetDistanceCm(REARRIGHT) > SIDERANGE; }
        bool ShortRearLeftFree()     { return sensors_reader_->GetDistanceCm(REARLEFT) > SHORTSIDERANGE; }
        bool ShortRearRightFree()    { return sensors_reader_->GetDistanceCm(REARRIGHT) > SHORTSIDERANGE; }
        bool ShortFrontLeftFree()    { return sensors_reader_->GetDistanceCm(FRONTLEFT) > SHORTSIDERANGE; }
        bool ShortFrontRightFree()   { return sensors_reader_->GetDistanceCm(FRONTRIGHT) > SHORTSIDERANGE; }

        bool LongRearLeftFree()     { return sensors_reader_->GetDistanceCm(REARLEFT) > LONGSIDERANGE; }
        bool LongRearRightFree()    { return sensors_reader_->GetDistanceCm(REARRIGHT) > LONGSIDERANGE; }
        bool LongFrontLeftFree()     { return sensors_reader_->GetDistanceCm(FRONTLEFT) > LONGSIDERANGE; }
        bool LongFrontRightFree()    { return sensors_reader_->GetDistanceCm(FRONTRIGHT) > LONGSIDERANGE; }


        // More fine-tuning For the front sensor
        bool StatFrontFree()        { return sensors_reader_->GetDistanceCm(FRONT) > FRONT_FREE_RANGE; }


        bool FrontFree()        { return sensors_reader_->GetDistanceCm(FRONT) > FRONT_FREE; }
        bool FrontClose()       { return sensors_reader_->GetDistanceCm(FRONT) <= FRONT_CLOSE; }
        bool FrontTooClose()    { return sensors_reader_->GetDistanceCm(FRONT) <= FRONT_TOO_CLOSE; }

        // Angle to the walls estimation
        double EstimateAngleToRightWall();
        double EstimateAngleToLeftWall();


    private:
        // sensors_reader_ provides all the data
        SensorsReader* sensors_reader_;

        // Mapping of the sensors
        static const int FRONT      = 4;
        static const int REAR       = 5;
        static const int FRONTLEFT  = 1;
        static const int REARLEFT   = 3;
        static const int FRONTRIGHT = 0;
        static const int REARRIGHT  = 2;

        // Tresholds
        static const int SIDERANGE = 24;    	// Min range to detect an opening
        static const int SHORTSIDERANGE = 15;
        static const int LONGSIDERANGE = 35;
        static const int SQUARESIZE = 40;   	// Length of the maze sections
        static const int FRONT_FREE = 32;   	// No obstacle detected in the front
        static const int FRONT_CLOSE = 20;  	// Wall detected
        static const int FRONT_TOO_CLOSE = 15;  // Too close to the wall

        static const int FRONT_FREE_RANGE = 28;

        // Angle estimation
        double estimate_angle_(int front_range, int rear_range);

    };
};

#endif /* COLLISION_DETECTOR_H_ */
